//
// Created by bobin on 17-11-15.
//

#include "G2OTypes.h"

namespace g2o {

    bool VertexSBALine::read(std::istream &is) {
        Vector6D lv;
        for (int i = 0; i < 6; i++)
            is >> _estimate[i];
        return true;
    }

    bool VertexSBALine::write(std::ostream &os) const {
        Vector6D lv = estimate();
        for (int i = 0; i < 6; i++) {
            os << lv[i] << " ";
        }
        return os.good();
    }

    void EdgeProjectLine::computeError() {
        const VertexSE3Expmap *v1 = static_cast<const VertexSE3Expmap *>(_vertices[1]);
        const VertexSBALine *v2 = static_cast<const VertexSBALine *>(_vertices[0]);
        const CameraParameters *cam
                = static_cast<const CameraParameters *>(parameter(0));
        Vector4D obs(_measurement);
        Vector6D est = v2->estimate();
        Vector2D u1 = cam->cam_map(v1->estimate().map(Vector3D(est[0], est[1], est[2])));
        Vector2D u2 = cam->cam_map(v1->estimate().map(Vector3D(est[3], est[4], est[5])));
        double dx = obs[2] - obs[0];
        double dy = obs[3] - obs[1];
        double n = hypot(dx, dy);
        dx /= n;
        dy /= n;
        double d = -dy * obs[0] + dx * obs[1];
        double dist1 = -dy * u1[0] + dx * u1[1] - d;
        double dist2 = -dy * u2[0] + dx * u2[1] - d;
        _error = Vector4D(dist1, dist2, 0, 0);
    }

    EdgeProjectLine::EdgeProjectLine() : BaseBinaryEdge<4, Vector4D, VertexSBALine, VertexSE3Expmap>() {
        _cam = 0;
        resizeParameters(1);
        installParameter(_cam, 0);
    }

    bool EdgeProjectLine::read(std::istream &is) {
        int paramId;
        is >> paramId;
        setParameterId(0, paramId);

        for (int i = 0; i < 4; i++) {
            is >> _measurement[i];
        }
        for (int i = 0; i < 4; i++)
            for (int j = i; j < 4; j++) {
                is >> information()(i, j);
                if (i != j)
                    information()(j, i) = information()(i, j);
            }
        return true;
    }

    bool EdgeProjectLine::write(std::ostream &os) const {
        os << _cam->id() << " ";
        for (int i = 0; i < 4; i++) {
            os << measurement()[i] << " ";
        }

        for (int i = 0; i < 4; i++)
            for (int j = i; j < 4; j++) {
                os << " " << information()(i, j);
            }
        return os.good();
        return true;
    }

    void EdgeLineSBAPoint::computeError() {
        const VertexSBALine *v1 = static_cast<const VertexSBALine *>(_vertices[0]);
        const VertexSBAPointXYZ *v2 = static_cast<const VertexSBAPointXYZ *>(_vertices[1]);

        Vector3D pt = v2->estimate();

        Vector6D line = v1->estimate();
        Vector3D lineStart = line.head<3>();
        Vector3D lineStartXY, lineStartXZ;
        lineStartXY << lineStart[0], lineStart[1], 1;
        lineStartXZ << lineStart[0], lineStart[2], 1;

        Vector3D lineEnd = line.tail<3>();
        Vector3D lineEndXY, lineEndXZ;
        lineEndXY << lineEnd[0], lineEnd[1], 1;
        lineEndXZ << lineEnd[0], lineEnd[2], 1;

        Vector3D plane_xy = lineStartXY.cross(lineEndXY).normalized();
        Vector3D plane_xz = lineStartXZ.cross(lineEndXZ).normalized();

        Vector3D ptxy, ptxz;
        ptxy << pt[0], pt[1], 1;
        ptxz << pt[0], pt[2], 1;
        _error[0] = ptxy.dot(plane_xy);
        _error[1] = ptxz.dot(plane_xz);
//            std::cout << _error << std::endl;

    }

    void EdgeLineSBAPoint::linearizeOplus() {
        const VertexSBALine *v1 = static_cast<const VertexSBALine *>(_vertices[0]);
        const VertexSBAPointXYZ *v2 = static_cast<const VertexSBAPointXYZ *>(_vertices[1]);

        Vector3D pt = v2->estimate();

        Vector6D line = v1->estimate();
        Vector3D lineStart = line.head<3>();
        Vector3D lineStartXY, lineStartXZ;
        lineStartXY << lineStart[0], lineStart[1], 1;
        lineStartXZ << lineStart[0], lineStart[2], 1;

        Vector3D lineEnd = line.tail<3>();
        Vector3D lineEndXY, lineEndXZ;
        lineEndXY << lineEnd[0], lineEnd[1], 1;
        lineEndXZ << lineEnd[0], lineEnd[2], 1;

        Vector3D plane_xy = lineStartXY.cross(lineEndXY);
        Vector3D plane_xz = lineStartXZ.cross(lineEndXZ);

        Vector3D ptxy, ptxz;
        ptxy << pt[0], pt[1], 1;
        ptxz << pt[0], pt[2], 1;
//        _error[0] = ptxy.dot(plane_xy);
//        _error[1] = ptxz.dot(plane_xz);
        _jacobianOplusXj.row(0) =  plane_xy.normalized();
        _jacobianOplusXj.row(1) =  plane_xz.normalized();

        _jacobianOplusXi.block<1, 3>(0, 0) =  (skew(lineEndXY).transpose() * ptxy);
        _jacobianOplusXi.block<1, 3>(0, 3) =  (skew(lineStartXY).transpose() * ptxy);
        _jacobianOplusXi.block<1, 3>(1, 0) =  (skew(lineEndXY).transpose() * ptxz);
        _jacobianOplusXi.block<1, 3>(1, 3) =  (skew(lineStartXY).transpose() * ptxz);

    }

    bool VertexPluckerLine::read(std::istream &is) {
        Vector6D lv;
        for (int i = 0; i < 6; i++)
            is >> _estimate[i];
        return true;
    }

    bool VertexPluckerLine::write(std::ostream &os) const {
        Vector6D lv = estimate();
        for (int i = 0; i < 6; i++) {
            os << lv[i] << " ";
        }
        return os.good();

    }

    EdgePluckerLine::EdgePluckerLine() {
        _cam = 0;
        resizeParameters(1);
        installParameter(_cam, 0);
    }

    bool EdgePluckerLine::read(std::istream &is) {
        int paramId;
        is >> paramId;
        setParameterId(0, paramId);

        for (int i = 0; i < 4; i++) {
            is >> _measurement[i];
        }
        for (int i = 0; i < 4; i++)
            for (int j = i; j < 4; j++) {
                is >> information()(i, j);
                if (i != j)
                    information()(j, i) = information()(i, j);
            }
        return true;
    }

    bool EdgePluckerLine::write(std::ostream &os) const {
        os << _cam->id() << " ";
        for (int i = 0; i < 4; i++) {
            os << measurement()[i] << " ";
        }

        for (int i = 0; i < 4; i++)
            for (int j = i; j < 4; j++) {
                os << " " << information()(i, j);
            }
        return os.good();
        return true;
    }

    void EdgePluckerLine::computeError() {
        const VertexSE3Expmap *v1 = static_cast<const VertexSE3Expmap *>(_vertices[1]);
        const VertexPluckerLine *v2 = static_cast<const VertexPluckerLine *>(_vertices[0]);
        const CameraParameters *cam
                = static_cast<const CameraParameters *>(parameter(0));
        Vector4D obs(_measurement);

        Vector3D start, end;
        start[2] = 1;
        end[2] = 1;
        start.head<2>() = ((obs.head<2>() - cam->principle_point) / cam->focal_length);
        end.head<2>() = ((obs.tail<2>() - cam->principle_point) / cam->focal_length);
        Vector6D est = v2->estimate();
        Vector3D line = start.cross(end);
        double a = line[0];
        double b = line[1];
        double c = line[2];

        PluckerMatrix matrix;
        SE3Quat se3Quat = v1->estimate();
        Matrix3D rot = se3Quat.rotation().normalized().toRotationMatrix();
        Vector3D trans = se3Quat.translation();
        matrix.block<3, 3>(0, 0) = rot;
        Matrix3D t_hat;
        t_hat << 0, -trans[2], trans[1],
                trans[2], 0, -trans[0],
                -trans[1], trans[0], 0;
        matrix.block<3, 3>(0, 3) = t_hat * rot;
        Vector3D line_est = matrix * est;

        double a_est = line_est[0];
        double b_est = line_est[1];
        double c_est = line_est[2];

        _error[0] = c * a_est - a * c_est;
        _error[1] = c * b_est - b * c_est;
        _error[2] = c * a_est - a * c_est;
        _error[3] = c * b_est - b * c_est;

    }

    void EdgePluckerLine::linearizeOplus() {
        const VertexSE3Expmap *v1 = static_cast<const VertexSE3Expmap *>(_vertices[1]);
        const VertexPluckerLine *v2 = static_cast<const VertexPluckerLine *>(_vertices[0]);
        const CameraParameters *cam
                = static_cast<const CameraParameters *>(parameter(0));
        Vector4D obs(_measurement);
        SE3Quat se3Quat(v1->estimate());
        PluckerMatrix matrix;
        Vector6D line = v2->estimate();
        Vector3D line_homo = line.tail<3>();
        Vector3D line_orth = line.head<3>();

        Vector3D start, end;
        start[2] = 1;
        end[2] = 1;
        start.head<2>() = ((obs.head<2>() - cam->principle_point) / cam->focal_length);
        end.head<2>() = ((obs.tail<2>() - cam->principle_point) / cam->focal_length);

        Vector3D x2dline = start.cross(end);
        double a = x2dline[0];
        double b = x2dline[1];
        double c = x2dline[2];

        Matrix3D rot = se3Quat.rotation().normalized().toRotationMatrix();
        Vector3D trans = se3Quat.translation();
        matrix.block<3, 3>(0, 0) = rot;
        Matrix3D t_hat = skew(trans);
        Matrix3D t_hat_r = t_hat * rot;
        matrix.block<3, 3>(0, 3) = t_hat_r;
        PluckerMatrix jac;
        jac.block<3, 3>(0, 0) = -skew(rot * line_orth) - t_hat * skew(rot * line_homo);
        jac.block<3, 3>(0, 3) = -skew(rot * line_homo);

        Vector3D line_est = matrix * line;

        double a_est = line_est[0];
        double b_est = line_est[1];
        double c_est = line_est[2];

        _error[0] = c * a_est - a * c_est;
        _error[1] = c * b_est - b * c_est;
        _error[2] = c * a_est - a * c_est;
        _error[3] = c * b_est - b * c_est;

        _jacobianOplusXj.row(0) =  _error[0] * (c * jac.row(0) - a * jac.row(2));
        _jacobianOplusXj.row(1) =  _error[1] * (c * jac.row(1) - b * jac.row(2));
        _jacobianOplusXj.row(2) =  _error[2] * (c * jac.row(0) - a * jac.row(2));
        _jacobianOplusXj.row(3) =  _error[3] * (c * jac.row(1) - a * jac.row(2));

    }

    void EdgePlaneSBAPoint::computeError() {
        const VertexSBAPointXYZ* v1 = static_cast<const VertexSBAPointXYZ *>(_vertices[1]);
        const VertexSBAPlane * v2 = static_cast<const VertexSBAPlane *>(_vertices[0]);

        Vector4D plane = v2->estimate();
        Vector4D point;
        point.head<3>() = v1->estimate();
        point[3] = 1;
        _error[0] = point.dot(plane);
    }

    void EdgePlaneSBAPoint::linearizeOplus() {
        const VertexSBAPointXYZ* v1 = static_cast<const VertexSBAPointXYZ *>(_vertices[1]);
        const VertexSBAPlane * v2 = static_cast<const VertexSBAPlane *>(_vertices[0]);

        Vector4D plane = v2->estimate().normalized();
        Vector3D point = v1->estimate();
        _jacobianOplusXj.row(0) = plane.head<3>();
        _jacobianOplusXi.block<1, 3>(0, 0) = point;
        _jacobianOplusXi(0, 3) =  1;
    }
} //namespace g2o